#include <Servo.h>
#define DEBUG 1
#include "Debug.h"


const uint32_t SERIAL_RATE = 9600;
const uint8_t speaker_pin = 8;

enum speaker_notes
{
	NOTE_A = 440, 
	NOTE_C = 262,
	NOTE_D = 294,
	NOTE_E = 145,

	NOTE_COUNT
};

enum servo_pin
{
	S0L = 5,
	S0R = 4,
	S1L = 7,
	S1R = 6,

	SERVO_COUNT
};

enum input_pin
{
	I0L = A1,
	I0R = A0,
	I1L = A2,
	I1R = A3,

	INPUT_COUNT
};

enum audio_pin
{
	A0L =  9,
	A0R = 12,
	A1L = 11,
	A1R = 10,

	AUDIO_COUNT
};



int32_t clamp(int32_t val, int32_t minv, int32_t maxv)
{
	return max(minv, min(maxv, val));
}


void playStartupTone()
{
	noTone(speaker_pin);
	tone(speaker_pin, NOTE_C, 200); delay(200);	noTone(speaker_pin);
	tone(speaker_pin, NOTE_D, 200); delay(200);	noTone(speaker_pin);
	tone(speaker_pin, NOTE_E, 200); delay(200); noTone(speaker_pin);
	tone(speaker_pin, NOTE_A, 200); delay(200); noTone(speaker_pin);
	tone(speaker_pin, NOTE_D, 200); delay(200); noTone(speaker_pin);
	noTone(speaker_pin);
}


void playCalibrationTone()
{
	noTone(speaker_pin);
	tone(speaker_pin, NOTE_C, 200); delay(200);	noTone(speaker_pin);
	delay(25);	noTone(speaker_pin);
	tone(speaker_pin, NOTE_C, 200); delay(200); noTone(speaker_pin);
}


void playReadyTone()
{
	noTone(speaker_pin);
	tone(speaker_pin, NOTE_C, 200); delay(200);	noTone(speaker_pin);
	tone(speaker_pin, NOTE_D, 200); delay(200);	noTone(speaker_pin);
	tone(speaker_pin, NOTE_D, 250); delay(250); noTone(speaker_pin);
	tone(speaker_pin, NOTE_A, 200); delay(200); noTone(speaker_pin);
	noTone(speaker_pin);
}


class FlowControl
{
public:
	FlowControl(uint8_t input_pin, uint8_t servo_pin)
	: instance(instances), input_pin(input_pin), servo_pin(servo_pin)
	{
		++instances;
		servo.attach(servo_pin, SERVO_DEFAULT_MIN_US, SERVO_DEFAULT_MAX_US);
	}


	FlowControl(uint8_t input_pin, uint8_t servo_pin, 
		uint16_t min_us, uint16_t max_us)
	: instance(instances), input_pin(input_pin), servo_pin(servo_pin)
	{
		++instances;
		servo.attach(servo_pin, min_us, max_us);
	}


	~FlowControl()
	{
		servo.detach();
	}


	void calibrate()
	{
		int8_t step = 1;
		uint8_t min_run[] = { 0, 0 };
		uint8_t max_run[] = { 0, 0 };

		min_run[0] = find_stop_angle(SERVO_DEFAULT_MIN_DEG, step);
		min_run[1] = find_open_angle(min_run[0], step);

		step = -1;
		max_run[0] = find_stop_angle(SERVO_DEFAULT_MAX_DEG, step);

		// smithre5: Kludgy hack ahead.
		if (instance == 1) {
			step = -step;
		}

		max_run[1] = find_open_angle(max_run[0], step);

		if (abs(max_run[1] - max_run[0]) < abs(min_run[1] - min_run[0])) {
			servo_extrema[0] = max_run[0];
			servo_extrema[1] = max_run[1];
		}
		else {
			servo_extrema[0] = min_run[0];
			servo_extrema[1] = min_run[1];
		}
		
		DEBUG_PRINT("FC");
		DEBUG_PRINT(instance);
		DEBUG_PRINT(": stop angle = ");
		DEBUG_PRINT(servo_extrema[0]);
		DEBUG_PRINT(", open angle = ");
		DEBUG_PRINTLN(servo_extrema[1]);

		servo.write(servo_extrema[0]);
		delay(SERVO_FULL_TRAVEL_DELAY);

		playCalibrationTone();
	}


	void set_rate(uint8_t rate)
	{
		uint8_t angle = map(rate, 0, 255, servo_extrema[0], servo_extrema[1]);

#if 0
		DEBUG_PRINT("FC");
		DEBUG_PRINT(instance);
		DEBUG_PRINT(": flow angle = ");
		DEBUG_PRINTLN(angle);
#endif

		servo.write(angle);
	}


	bool get_input()
	{
		return analogRead(input_pin) > INPUT_THRESHOLD;
	}


public:
	static const uint16_t	SERVO_DEFAULT_MIN_US	=  440;
	static const uint16_t	SERVO_DEFAULT_MAX_US	= 1800;

	static const uint8_t	SERVO_DEFAULT_MIN_DEG	=   0;
	static const uint8_t	SERVO_DEFAULT_MAX_DEG	= 180;
	static const uint8_t	SERVO_EXTREMA_SZ		=   2;

	// Values in milliseconds.
	static const uint16_t	SERVO_FULL_TRAVEL_DELAY	=  2500;
	static const uint16_t	SERVO_STEP_TRAVEL_DELAY =    50;
	static const uint16_t	SERVO_MIN_FREQ_DELAY	=     1;
	static const uint16_t	SERVO_MAX_FREQ_DELAY	= 12000;

	static const uint32_t	SERVO_OPEN_THRESHOLD	= 50000;

	static const float		INPUT_THRESHOLD			= 1.75f;

	static uint16_t			instances;


private:
	FlowControl() {}
	FlowControl(const FlowControl& copy) {}



	uint8_t find_stop_angle(uint8_t angle, int8_t& step)
	{
		DEBUG_PRINT("FC");
		DEBUG_PRINT(instance);
		DEBUG_PRINTLN(": seeking stop angle");

		uint8_t clamped_angle = 0;
		uint8_t coarse_angle = 0;
		int8_t coarse_step = step;

		servo.write(angle);
		delay(SERVO_FULL_TRAVEL_DELAY);

		// Quick step near to extreme.
		while (analogRead(input_pin) < INPUT_THRESHOLD) {
			angle = angle + step;

			clamped_angle = 
				clamp(angle, SERVO_DEFAULT_MIN_DEG, SERVO_DEFAULT_MAX_DEG);

			if (clamped_angle != angle) {
				step = -step;
			}

			servo.write(angle);
			delay(SERVO_STEP_TRAVEL_DELAY);
		}

		coarse_angle = angle;
		coarse_step = step;

		DEBUG_PRINT("FC");
		DEBUG_PRINT(instance);
		DEBUG_PRINTLN(": found coarse stop angle");

		// Back off from coarse extrema.
		bool dripped = false;

		while (true) {
			unsigned long timerStart = millis();
			unsigned long timerStop = timerStart;
			dripped = false;

			while ((timerStop - timerStart) < SERVO_MAX_FREQ_DELAY) {
				if (analogRead(input_pin) > INPUT_THRESHOLD) {
					dripped = true;
					break;
				}

				timerStop = millis();
			}

			if (!dripped) {
				break;
			}

			angle = angle - step;

			DEBUG_PRINT("FC");
			DEBUG_PRINT(instance);
			DEBUG_PRINT(": backstep to ");
			DEBUG_PRINTLN(angle);

			clamped_angle = 
				clamp(angle, SERVO_DEFAULT_MIN_DEG, SERVO_DEFAULT_MAX_DEG);

			if (clamped_angle != angle) {
				angle = coarse_angle;
				step = -step;
			}

			servo.write(angle);
			delay(SERVO_STEP_TRAVEL_DELAY);
		}

		DEBUG_PRINT("FC");
		DEBUG_PRINT(instance);
		DEBUG_PRINTLN(": found fine stop angle");

		step = coarse_step;

		return angle;
	}


	uint8_t find_open_angle(uint8_t angle, int8_t& step)
	{
		DEBUG_PRINT("FC");
		DEBUG_PRINT(instance);
		DEBUG_PRINTLN(": seeking open angle");

		servo.write(angle);
		delay(SERVO_FULL_TRAVEL_DELAY);

		while (analogRead(input_pin) < INPUT_THRESHOLD) {
			angle = angle + step;
			servo.write(angle);
			delay(SERVO_STEP_TRAVEL_DELAY);
		}

		for (uint32_t i = 0, j = 0; i < SERVO_OPEN_THRESHOLD;) {
			if (analogRead(input_pin) < INPUT_THRESHOLD) {
				if (j > i) {
					step = -step;
				}
				
				angle = angle + step;
				servo.write(angle);
				delay(SERVO_STEP_TRAVEL_DELAY);

				j = i;
				i = 0;
			}
			else {
				++i;
			}
		}

		DEBUG_PRINT("FC");
		DEBUG_PRINT(instance);
		DEBUG_PRINT(": found open angle ");
		DEBUG_PRINTLN(angle);

		return angle;
	}


private:
	uint16_t	instance;

	uint8_t		input_pin;
	uint8_t		servo_pin;
	Servo		servo;

	uint8_t		servo_extrema[SERVO_EXTREMA_SZ];
};


uint16_t FlowControl::instances = 0;
FlowControl* controls[4];
bool note_play = false;


void setup()
{
	pinMode(speaker_pin, OUTPUT);

	pinMode(A0L, INPUT);
	pinMode(A0R, INPUT);
	pinMode(A1L, INPUT);
	pinMode(A1R, INPUT);

	pinMode(I0L, INPUT);
	pinMode(I0R, INPUT);
	pinMode(I1L, INPUT);
	pinMode(I1R, INPUT);

	pinMode(S0L, OUTPUT);
	pinMode(S0R, OUTPUT);
	pinMode(S1L, OUTPUT);
	pinMode(S1R, OUTPUT);

	Serial.begin(SERIAL_RATE);

	playStartupTone();
	DEBUG_PRINTLN("Calibration start.");

	//controls[0] = new FlowControl(I0L, S0L);
	//controls[0]->calibrate();
	//controls[0]->set_rate(0);

	controls[1] = new FlowControl(I0R, S0R);
	controls[1]->calibrate();
	controls[1]->set_rate(0);

	controls[2] = new FlowControl(I1L, S1L);
	controls[2]->calibrate();
	controls[2]->set_rate(0);

	//controls[3] = new FlowControl(I1R, S1R);
	//controls[3]->calibrate();
	//controls[3]->set_rate(0);

	playReadyTone();
	DEBUG_PRINTLN("Calibration complete.");
}


void loop()
{
	note_play = false;

	if (digitalRead(A0L)) {
		controls[0]->set_rate(255);
		DEBUG_PRINTLN("A0L");
	}
	else {
		controls[0]->set_rate(0);
	}

	if (controls[0]->get_input()) {
		tone(speaker_pin, NOTE_A);
		note_play = true;
	}
#if 0
	if (digitalRead(A0R)) {
		controls[1]->set_rate(255);
	}
	else {
		controls[1]->set_rate(0);
	}

	if (controls[1]->get_input()) {
		tone(speaker_pin, NOTE_C);
		note_play = true;
	}

	if (digitalRead(A1L)) {
		controls[2]->set_rate(255);
	}
	else {
		controls[2]->set_rate(0);
	}

	if (controls[2]->get_input()) {
		tone(speaker_pin, NOTE_D);
		note_play = true;
	}
#endif
	if (digitalRead(A1R)) {
		controls[3]->set_rate(255);
	}
	else {
		controls[3]->set_rate(0);
	}

	if (controls[3]->get_input()) {
		tone(speaker_pin, NOTE_E);
		note_play = true;
	}

	if (!note_play) {
		noTone(speaker_pin);
	}
}

